/* 
 * File:   CollisionHandler.cpp
 * Author: RedEyedKiller
 * 
 * Created on 10 Απρίλιος 2011, 1:27 μμ
 */

#include <algorithm>
#include <sstream>

#include "CollisionHandler.h"
#include "../parsers/LevelParser.h"
#include "../TileProperties.h"
#include "../Logger.h"
#include "../TimedUpdater.h"
#include "../ScriptLogic.h"
#include "../LevelMap.h"
#include "EntityContactInfo.h"
#include "Contact.h"
#include "PhysicsManager.h"

#ifdef DEBUG_DRAW
#include "../sdl/Color.h"
#endif

/**
 * A limit of the buffer used by the tile-entity algorithm.
 * It mustn't be less than (max_width_of_entity / tile_width).
 */
const unsigned short VERTICAL_BUFFER_LIMIT = 5;

namespace physicsSystem
{
namespace PhysicsInternals
{

CollisionHandler::CollisionHandler(int entityollisionUpdateTime) : RDC_SIZE_LIMIT(10), ITERAT_MAX_MOD(20), SEP_VEL_TOL(-0.2), PENET_TOL(0.5)
{
    this->entityollisionUpdateTime = entityollisionUpdateTime;
}

CollisionHandler::~CollisionHandler()
{
}

/**
 * All collision checks are performed in this method.
 */

void CollisionHandler::CollisionCheck(PhysicsPool& pool, LevelMap* currentMap, TileProperties* properties, float duration)
{
    static TimedUpdater tEntities;
    this->duration = duration;
    if( tEntities.Update( 0 ) )
    {
        //sort the physics pool and make 
        //any other needed operation to all of
        //physics logic objects
        PreaparePool( pool );
        //find any collision that has occureded
        EntityPerEntityRDC( pool, 0, AXIS_X, AXIS_X, AXIS_Y );
        //check entities against the tile map
        EntityPerTileCheck( currentMap, pool, properties );
        //and resolve them
        ColisionResolution( duration );
    }
}

/**
 * Resolves all entity per entity collsion events.
 */
void CollisionHandler::ColisionResolution(float duration)
{
    using namespace EntitySystem;
    int iterations;
    const int size = entityCollisionBuffer.size( );
    const int iterationsMax = size * ITERAT_MAX_MOD;

    //optim TEST
    CompareContacts cmp;
    std::sort( entityCollisionBuffer.begin( ), entityCollisionBuffer.end( ), cmp );

    for( iterations = 0; iterations < iterationsMax; iterations++ )
    {
        float max = 999999;
        Contact* maxContact = NULL;
        for( int j = 0; j < size; j++ )
        {
            float sepVel = entityCollisionBuffer[j].GetSeparatingVelocity( );
            if( sepVel < max && ( sepVel < SEP_VEL_TOL || entityCollisionBuffer[j].GetPenetration( ) > PENET_TOL ) )
            {
                max = sepVel;
                maxContact = &entityCollisionBuffer[j];
            }
        }

        //if no contact to be resolved was found
        //exit the algorithm
        if( maxContact == NULL )
            break;

        maxContact->Resolve( duration );
        //update th movement of all contacts
        Math::Vector2F * const move = maxContact->GetMovementArray( );

        if( move[0] == move[1] && move[0] == Math::Vector2F::Zero )
            continue;

        //possible optim keep track of how many contacts each physics logic object has
        for( int i = 0; i < size; i++ )
        {
            Contact* contact = &entityCollisionBuffer[i];
            //check if the contacs belong in the same group
            if( contact->GetGroup( ) < maxContact->GetGroup( ) )
                continue;
            else if( contact->GetGroup( ) > maxContact->GetGroup( ) )
                break;

            if( contact->GetFirst( ) == maxContact->GetFirst( ) )
            {
                contact->AdjustPenetration( -move[0].DotProduct( contact->GetContactNormal( ) ) );
            }
            else if( contact->GetFirst( ) == maxContact->GetSecond( ) )
            {
                contact->AdjustPenetration( -move[1].DotProduct( contact->GetContactNormal( ) ) );
            }

            if( contact->GetSecond( ) )
            {
                if( contact->GetSecond( ) == maxContact->GetFirst( ) )
                {
                    contact->AdjustPenetration( move[0].DotProduct( contact->GetContactNormal( ) ) );
                }
                else if( contact->GetSecond( ) == maxContact->GetSecond( ) )
                {
                    contact->AdjustPenetration( move[1].DotProduct( contact->GetContactNormal( ) ) );
                }
            }
        }
    }

    for( int i = 0; i < size; i++ )
    {
        entityCollisionBuffer[i].ReportScripts( );
    }

#ifdef DEBUG_DRAW 
    std::ostringstream oss;
    oss << iterations << " , " << entityCollisionBuffer.size( );
    RenderingManager::GetInstance( )->RenderText( Math::Vector2F( 0, 50 ), oss.str( ) );
#endif  
    entityCollisionBuffer.clear( );
}

void CollisionHandler::TileCollision(const TileColEvent& event, EntitySystem::PhysicsLogic* pObject, TileProperties* properties)
{
    using Math::Vector2F;

    //get collision vector
    Vector2F contactNormal, info;
    {
        Math::Rect pollingRect1( *( pObject->GetRect( ) ) );
        pollingRect1.SetOrigin( pObject->GetPosition( ).ConvertTo<int>( ) );
        pollingRect1.Intersect( event.region, info );

        Math::Vector2I commonPoints[2];

        if( !Math::CommonPoints( pollingRect1, event.region, commonPoints ) )
        {
            Math::CommonPoints( event.region, pollingRect1, commonPoints );
        }

        contactNormal = ( commonPoints[0] - commonPoints[1] ).ConvertTo<float>( );

        //flip members and get their absolute values
        contactNormal.Absolute( );
        //and flip the members
        contactNormal.Flip( );

        //nulify the lesser one
        if( contactNormal.GetX( ) > contactNormal.GetY( ) )
        {
            contactNormal.Set( 1.0f, 0.0f );
        }
        else if( contactNormal.GetX( ) < contactNormal.GetY( ) )
        {
            contactNormal.Set( 0.0f, 1.0f );
        }
        else
        {
            contactNormal.Set( 0.5f, 0.5f );
        }

        Math::Vector2F dC = pollingRect1.GetCenter( ) - event.region.GetCenter( );

        dC *= contactNormal;

        dC.Normalize( );

        contactNormal *= dC;
    }

    int numOfLoops = 0;
    unsigned int mask = 1 << numOfLoops;
    unsigned int flag = event.tile;
    while( flag != 0 )
    {
        if( flag & 1 )//check if it is solid
        {
            std::string tile( "Solid" );
            pObject->OnTileCollision( tile, contactNormal );
            entityCollisionBuffer.push_back( Contact( pObject, info, contactNormal ) );

            flag ^= 1; //remove event type from mask.
        }
        if( flag == mask )
        {
            std::string tile( properties->NameOf( flag ) );
            //run script.
            pObject->OnTileCollision( tile, contactNormal );
            flag ^= mask; //remove event type from mask.
        }
        numOfLoops++;
        mask = 1 << numOfLoops; //calculate next mask.
    }
}

void CollisionHandler::TilesCollisionResolution(EntitySystem::PhysicsLogic* pObject, const std::vector<TileColEvent>& eventBuffer, TileProperties* properties)
{
    using namespace EntitySystem;

    if( eventBuffer.empty( ) )
    {
        static std::string none( "NONE" );
        pObject->OnTileCollision( none, Math::Vector2F::Zero );
    }
    else
    {
        std::vector<TileColEvent>::const_iterator eventIt = eventBuffer.begin( );
        for(; eventIt != eventBuffer.end( ); ++eventIt )
        {

#ifdef DEBUG_DRAW
            eventIt->region;
            RenderingManager::GetInstance( )->RenderRect( eventIt->region.GetOrigin( ).ConvertTo<float>( ), eventIt->region.GetWidth( ),
                                                          eventIt->region.GetHeight( ), 255, 255, 0 );
            RenderingManager::GetInstance( )->RenderRect( eventIt->region.GetOrigin( ).ConvertTo<float>( ), eventIt->region.GetWidth( ),
                                                          eventIt->region.GetHeight( ), 255, 255, 0 );
#endif
            TileCollision( *eventIt, pObject, properties );

        }
    }
}

/**
 * Check which pairs of entities collide and fills up the entityCollisionBuffer.
 * This is the brute force step of RDC. Instead of using a pure brute force algorithm
 * this method takes andvantage of the (required by the RDC) sorted list.
 * @param group
 * @param sortAxis - the axis the last sort happened.
 */
void CollisionHandler::BruteForceEntityCheck(PhysicsPool& group, Axis sortAxis, int groupIndex)
{
    PhysicsPool::iterator it = group.begin( );
    PhysicsPool::iterator end = group.end( );
    //A vector to store the collision difference for each collision test.
    Math::Vector2F axisInfo( 0, 0 );
    for(; it != end; ++it )//for each item in the group (even the last one in order to set up the groupIndex)
    {
        EntitySystem::PhysicsLogic* current = *it;
        current->SetGroupIndex( groupIndex );

        //Get the next phtsical object.(Its no use to check current with itself)
        PhysicsPool::iterator toCheck = it + 1;

        Math::Rect checkRect( *( current->GetRect( ) ) );

        //For optimal execution.
        //This threashold describes the last pixel in the sortAxis that this
        //physical object occupies.
        //If a physical object is found that surpasses this threashold then
        //there is no way that this two objects collided.
        //And since the physics pool is sorted by the position in the sortAxis
        //if one to be checked object surpasses the threashold all other
        //to be checked objects are above the threashold.
        int threashold = ( AXIS_X == sortAxis ) ? checkRect.GetTotalWidth( ) : checkRect.GetTotalHeight( );
        //check all remaining items if their sortAxis position is less than
        //the TotalDimension of current.

        while( toCheck != end )//&& (*toCheck)->GetPosition().GetX() <= threashold)
        {
            Math::Rect tobeChecked( ( *( *toCheck )->GetRect( ) ) );

            //take andvantage of the sorting
            int limit = ( AXIS_X == sortAxis ) ? tobeChecked.GetLeft( ) : tobeChecked.GetTop( );
            if( limit > threashold )break;

            if( !current->IsAwake( ) && !( *toCheck )->IsAwake( ) )
            {
                ++toCheck;
                continue;
            }

            Math::IntersectionType result = checkRect.Intersect( tobeChecked, axisInfo );
            if( result != Math::INTER_NO )
            {//collision found, write it down on the colBuffer
                entityCollisionBuffer.push_back( Contact( current, *toCheck, axisInfo, groupIndex ) );
            }
            //fetch the next object to be checked.
            ++toCheck;
        }
    }
}

/**
 * The core of the RDC  ( Recursive Dimensional Clustering (wow!) ) algorithm.
 * It analyzes the current group on the given axis.
 * If the group is canot divede by the given axis proceeds to the next one.
 * If the group can be divided breaks it into two groups and for each group 
 * restarts the algorithm.
 * If the given currentAxis is Invalid or the group is to small BruteForce is called 
 * to finish up the work.
 * 
 * @param group
 * @param lastAxis - Used to determine sortAxis for BruteForceEntityCheck. It the last currentAxis.
 * @param currentAxis - The axis for the current step
 * @param futureAxis - The axis fot the next step.
 */
int CollisionHandler::EntityPerEntityRDC(PhysicsPool& group, int groupIndex, Axis lastAxis, Axis currentAxis, Axis futureAxis)
{
    if( group.size( ) < RDC_SIZE_LIMIT || currentAxis == AXIS_INVALID )
    {
        BruteForceEntityCheck( group, lastAxis, groupIndex );
    }
    else
    {
        BoundaryList boundaryList;
        //create the boundary list
        InitializeBoundaryList( group, boundaryList, currentAxis );
        //and sort it out
        SortBoundaryList( boundaryList );

        int count = 0;
        PhysicsPool subGroup;

        Axis ax1 = futureAxis;
        Axis ax2 = AXIS_INVALID;
        bool subdivision = false;

        BoundaryList::iterator it = boundaryList.begin( );
        BoundaryList::const_iterator end = boundaryList.end( );
        for(; it != end; ++it )//for each element of the group
        {
            if( it->open )
            {
                count++;
                //add to the new Group
                subGroup.push_back( it->object );
            }
            else
            {
                count--;
                if( count == 0 )//end of a cluster
                {
                    if( it != --boundaryList.end( ) )//subdivision
                    {
                        subdivision = true;
                    }
                    //if we divided this group in the past
                    if( subdivision )
                    {
                        if( ax1 == AXIS_X )
                            ax1 = AXIS_Y;
                        else
                            ax1 = AXIS_X;
                    }
                    //no need to call it for one man group
                    if( subGroup.size( ) != 1 )
                    {
                        groupIndex++;
                        groupIndex = EntityPerEntityRDC( subGroup, groupIndex, currentAxis, ax1, ax2 ) + 1;
                    }
                    subGroup.clear( );
                }
            }
        }

    }
    return groupIndex;
}

/**
 * Sorts the given boundary list.
 * @param list
 */
void CollisionHandler::SortBoundaryList(BoundaryList& list)
{
    CompareBoundaryEntries cmp;
    std::stable_sort( list.begin( ), list.end( ), cmp );
}

/**
 * Creates the boundarylist of the given group on the given axis.
 * 
 * A boundary list for each object in the group creates two entires:
 * One OPEN with the the minimum point of the object on the given axis.
 * One CLOSE with the maximum point of the object on the given axis.
 * 
 * @param group
 * @param list
 * @param axis - The axis on which the list will be made.
 */
void CollisionHandler::InitializeBoundaryList(PhysicsPool& group, BoundaryList& list, Axis axis)
{
    PhysicsPool::iterator it = group.begin( );
    PhysicsPool::const_iterator end = group.end( );
    for(; it != end; ++it )
    {
        EntitySystem::PhysicsLogic* current = *it;
        //we need to create two boundary list entries
        //one at the begining of the object and one at the end
        //and specify the first as open and the scond one as !open
        RDCBoundaryEntry open, close;
        open.open = true;
        close.open = false;

        //pick the position accordingly to the axis
        if( axis == AXIS_X )
        {
            open.posInAxis = current->GetRect( )->GetLeft( );
            close.posInAxis = current->GetRect( )->GetTotalWidth( );
        }
        else
        {
            open.posInAxis = current->GetRect( )->GetTop( );
            close.posInAxis = current->GetRect( )->GetTotalHeight( );
        }

        open.object = close.object = ( *it );
        //push back both entries
        list.push_back( open );
        list.push_back( close );
    }
}

/**
 * Checks for each entity if it collides with a tile. Afterwrds TileCollisionResolution is called
 * to handle the collision events if any.
 * @param currentMap - The map against which the handler will check.
 * @param pool - The container with all entities. (sorting is not required)
 * @param properties - The object that describes the properties of tiles.
 */
void CollisionHandler::EntityPerTileCheck(LevelMap* currentMap, PhysicsPool& pool, TileProperties* properties)
{
    if( currentMap == NULL ) return;
    using namespace EntitySystem;
    const int tileH = currentMap->GetTileHeight( ), tileW = currentMap->GetTileWidth( );
    PhysicsPool::iterator it = pool.begin( );
    PhysicsPool::iterator end = pool.end( );

    for(; it != end; ++it )//for each entity
    {
        EntitySystem::PhysicsLogic* current = *it;

        if( !current->IsAwake( ) ) continue;

        {
            const Math::Rect* r = current->GetRect( );
            //calculate the first and last tile of the map current touches
            const int startX = current->GetPosition( ).GetX( ) / tileW;
            const int endX = r->GetTotalWidth( ) / tileW;
            const int startY = current->GetPosition( ).GetY( ) / tileH;
            const int endY = r->GetTotalHeight( ) / tileH;

            if( startX < 0 || startY < 0 ) continue;

#ifdef DEBUG_DRAW
            RenderingManager::GetInstance( )->RenderRect( Math::Vector2F( startX * 64, startY * 64 ), ( endX - startX + 1 ) * 64,
                                                          ( endY - startY + 1 ) * 64, 255, 0, 0 );
#endif
            //the buffer we will store the collision events.
            std::vector<TileColEvent> eventBuffer;
            if( endY - startY + 1 > VERTICAL_BUFFER_LIMIT )
                Logger::GetInstance( )->Get( Logger::CHANNEL_PHYSICS ) << "Tile number larger than VerticalBufferLimt";

            unsigned int lastVerticalFlag[VERTICAL_BUFFER_LIMIT];
            memset( lastVerticalFlag, 0, sizeof(unsigned int ) * VERTICAL_BUFFER_LIMIT );
            unsigned int lastHorizontalFlag = 0;
            TileColEvent verticalRegion[VERTICAL_BUFFER_LIMIT], horizontialRegion;

            // <editor-fold defaultstate="collapsed" desc="The Great Loop">
            for( int yTile = startY; yTile <= endY; yTile++ )//for each row
            {
                for( int xTile = startX; xTile <= endX; xTile++ )//for each column
                {
                    //get the combined flag of all tiles at i,j
                    unsigned long flag = currentMap->GetTilelistProperties( xTile, yTile );
                    if( flag )
                    {
                        // <editor-fold defaultstate="collapsed" desc="Handle horizontal events">
                        if( lastHorizontalFlag == 0 )//set up the new event
                        {
                            lastHorizontalFlag = horizontialRegion.tile = flag;
                            horizontialRegion.region.Set( yTile * tileH, xTile * tileW, tileH, tileW );
                        }
                        else if( flag == lastHorizontalFlag )//expand the old one
                        {
                            //add to region
                            horizontialRegion.region.SetWidth( horizontialRegion.region.GetWidth( ) + tileW );
                        }
                        else//submit the current one and create a new one
                        {
                            //check if we can comine the old flag with the new one
                            if( horizontialRegion.tile & flag )//if the current flag exists inside the new one
                            {
                                //add to region
                                horizontialRegion.region.SetWidth( horizontialRegion.region.GetWidth( ) + tileW );
                            }
                            else
                            {
                                if( horizontialRegion.tile != 0 )
                                    eventBuffer.push_back( horizontialRegion );
                                //create a new one
                                horizontialRegion.tile = flag;
                                horizontialRegion.region.Set( yTile * tileH, xTile * tileW, tileH, tileW );
                            }


                        }// </editor-fold>

                        // <editor-fold defaultstate="collapsed" desc="Handle vertical events">
                        if( lastVerticalFlag[xTile - startX] == 0 )
                        {
                            verticalRegion[xTile - startX].tile = flag;
                            verticalRegion[xTile - startX].region.Set( yTile * tileH, xTile * tileW, tileH, tileW );
                        }
                        else if( flag == lastVerticalFlag[xTile - startX] )
                        {
                            verticalRegion[xTile - startX].region.SetHeight( verticalRegion[xTile - startX].region.GetHeight( ) + tileH );
                        }
                        else
                        {
                            //check if it is safe to add to old region or ...
                            if( verticalRegion[xTile - startX].tile & flag )
                            {
                                verticalRegion[xTile - startX].region.SetHeight( verticalRegion[xTile - startX].region.GetHeight( ) + tileH );
                            }
                            else
                            {
                                //submit old region
                                if( verticalRegion[xTile - startX].tile != 0 )
                                    eventBuffer.push_back( verticalRegion[xTile - startX] );
                                //create a new one
                                lastVerticalFlag[xTile - startX] = verticalRegion[xTile - startX].tile = flag;
                                verticalRegion[xTile - startX].region.Set( yTile * tileH, xTile * tileW, tileH, tileW );
                            }
                        }// </editor-fold>
                    }
                    else
                    {
                        // <editor-fold defaultstate="collapsed" desc="Submit">
                        if( horizontialRegion.tile != 0 && horizontialRegion.region.GetWidth( ) > tileW )
                        {
                            eventBuffer.push_back( horizontialRegion );
                            horizontialRegion.tile = 0;
                        }
                        else if( lastVerticalFlag[xTile - startX] != 0 && verticalRegion[xTile - startX].region.GetHeight( ) > tileH )
                        {
                            eventBuffer.push_back( verticalRegion[xTile - startX] );
                            verticalRegion[xTile - startX].tile = 0;
                        }
                        // </editor-fold>
                    }
                    lastHorizontalFlag = lastVerticalFlag[xTile - startX] = flag;
                }

                lastHorizontalFlag = 0;
            }// </editor-fold>
            FinalizeTileCollisionDetection( eventBuffer, horizontialRegion, verticalRegion, endX - startX, tileW );
            TilesCollisionResolution( current, eventBuffer, properties );
            eventBuffer.clear( );
        }
    }
}

/**
 * Finalizes the EntityPerTileCheck by pushing the remaining events.
 */
void CollisionHandler::FinalizeTileCollisionDetection(std::vector<TileColEvent>& eventBuffer,
                                                      const TileColEvent& horizontialRegion,
                                                      TileColEvent * const verticalRegion, int colms, int tileW)
{
    //check if there is a wothmentioning horizontial event
    if( horizontialRegion.tile != 0 && horizontialRegion.region.GetWidth( ) > tileW )
    {
        eventBuffer.push_back( horizontialRegion );
        for( int i = 0; i <= colms; i++ )//for each column
        {
            if( verticalRegion[i].tile != 0 )
            {
                //;exclude the above mentioned horizontial event
                if( verticalRegion[i].region.GetTop( ) != horizontialRegion.region.GetTop( ) )
                    eventBuffer.push_back( verticalRegion[i] );
            }
        }
    }
    else
    {//no horizontial event was found so push all vertical events
        for( int i = 0; i <= colms; i++ )//for each column
        {
            if( verticalRegion[i].tile != 0 )
            {
                eventBuffer.push_back( verticalRegion[i] );
            }
        }
    }
}

/**
 * Performs all required operations in order to begin entity per entity collision checks.
 * --Sorts the physics pool so that objects with smaller x position will be on the top
 * of the vector.
 */
void CollisionHandler::PreaparePool(PhysicsPool& pool)
{
    //sort the physics pool by the x position only if the rdc will 
    if( pool.size( ) < RDC_SIZE_LIMIT )
    {
        ComparePhysics comp;
        std::sort( pool.begin( ), pool.end( ), comp );
    }
    //reset each object
    Reseter r;
    std::for_each( pool.begin( ), pool.end( ), r );
}


}
}
